#include "Base_Function.h"

int left_chassis_motor_forward = 9;
int left_chassis_motor_backward = 8;
int right_chassis_motor_forward = 10;
int right_chassis_motor_backward = 12;
const int SensorLeft_2 = A1;
const int SensorRight_2 = A0;
const int SensorLeft_1 = A3;
const int SensorRight_1 = A2;
int SL_2; // 循迹
int SR_2;
int SL_1; // 避障
int SR_1;

void BF_Init(void)
{
    pinMode(left_chassis_motor_forward, OUTPUT);
    pinMode(left_chassis_motor_backward, OUTPUT);
    pinMode(right_chassis_motor_forward, OUTPUT);
    pinMode(right_chassis_motor_backward, OUTPUT);

    pinMode(SensorLeft_2, INPUT);
    pinMode(SensorRight_2, INPUT);
    pinMode(SensorLeft_1, INPUT);
    pinMode(SensorRight_1, INPUT);
}

void chassis_accelerate(int speed)
{
    analogWrite(left_chassis_motor_forward, speed);
    digitalWrite(left_chassis_motor_backward, LOW);
    analogWrite(right_chassis_motor_forward, speed);
    digitalWrite(right_chassis_motor_backward, LOW);
}

void chassis_brake(void)
{
    analogWrite(left_chassis_motor_forward, 0);
    digitalWrite(left_chassis_motor_backward, LOW);
    analogWrite(right_chassis_motor_forward, 0);
    digitalWrite(right_chassis_motor_backward, LOW);
}

void chassis_leftturn(int speed)
{
    analogWrite(left_chassis_motor_forward, 0);
    digitalWrite(left_chassis_motor_backward, LOW);
    analogWrite(right_chassis_motor_forward, speed);
    digitalWrite(right_chassis_motor_backward, LOW);
}

void chassis_rightturn(int speed)
{
    analogWrite(left_chassis_motor_forward, speed);
    digitalWrite(left_chassis_motor_backward, LOW);
    analogWrite(right_chassis_motor_forward, 0);
    digitalWrite(right_chassis_motor_backward, LOW);
}

void leftturn(int time, int speed)
{
    analogWrite(left_chassis_motor_forward, 0);
    digitalWrite(left_chassis_motor_backward, LOW);
    analogWrite(right_chassis_motor_forward, speed);
    digitalWrite(right_chassis_motor_backward, LOW);
    delay(time * 16);
}

void rightturn(int time, int speed)
{
    analogWrite(left_chassis_motor_forward, speed);
    digitalWrite(left_chassis_motor_backward, LOW);
    analogWrite(right_chassis_motor_forward, 0);
    digitalWrite(right_chassis_motor_backward, LOW);
    delay(time * 16);
}